#!/usr/bin/env python3
# -*- coding: utf-8 -*-

from LOBOROBOT import LOBOROBOT
import RPi.GPIO as GPIO
import time


def calculate_duration(distance_m, speed_percent):
    """根据实测数据计算行驶时间"""
    # 校准参数（需要根据实际测试调整）
    # 假设60%速度下每秒行进0.35米
    base_speed = 0.35  # 单位：米/秒（在60% PWM时的速度）
    actual_speed = base_speed * (speed_percent / 60.0)
    return distance_m / actual_speed


if __name__ == "__main__":
    bot = LOBOROBOT()
    try:
        # 运行参数
        target_distance = 2.0  # 目标距离：2米
        run_speed = 60  # 速度百分比（建议60%）

        # 计算持续时间
        run_time = calculate_duration(target_distance, run_speed)
        print(f"开始前进 {target_distance} 米，预计耗时 {run_time:.1f} 秒")

        # 执行前进动作
        bot.t_up(run_speed, run_time)

        # 等待动作完成
        time.sleep(run_time + 0.5)  # 增加0.5秒缓冲

        print("到达目标位置！")

    except KeyboardInterrupt:
        print("\n用户中断操作")
    finally:
        bot.t_stop(0)
        GPIO.cleanup()
        print("GPIO已清理")